from datetime import datetime

import RPi.GPIO as GPIO
import time

# 小车电机引脚定义
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13

# 小车状态值定义
enSTOP = 0
enRUN = 1
enBACK = 2
enLEFT = 3
enRIGHT = 4
enTLEFT = 5
enTRIGHT = 6

# 小车舵机定义
enFRONTSERVOLEFT = 1
enFRONTSERVORIGHT = 2
# 摄像头舵机
enSERVOLEFT = 3
enSERVORIGHT = 4
enSERVOSTOP = 5
enSERVOUP = 6
enSERVODOWN = 7
enSERVO_Vertical_Stop = 8

# 初始化摄像头舵机上下左右角度为90度
ServoHorizontalPos = 90
ServoVerticalPos = 90
g_frontServoPos = 90
g_nowfrontPos = 0

# 小车按键定义
key = 8

# 超声波引脚定义
EchoPin = 0
TrigPin = 1

# RGB三色灯引脚定义
LED_R = 22
LED_G = 27
LED_B = 24

# 舵机引脚定义
FrontServoPin = 23
ServoVerticalPin = 9
ServoHorizontalPin = 11

# 红外避障引脚定义
AvoidSensorLeft = 12
AvoidSensorRight = 17

# 蜂鸣器引脚定义
buzzer = 8

# 灭火电机引脚设置
OutfirePin = 2

# 循迹红外引脚定义
# TrackSensorLeftPin1 TrackSensorLeftPin2 TrackSensorRightPin1 TrackSensorRightPin2
#      3                 5                  4                   18
TrackSensorLeftPin1 = 3  # 定义左边第一个循迹红外传感器引脚为3口
TrackSensorLeftPin2 = 5  # 定义左边第二个循迹红外传感器引脚为5口
TrackSensorRightPin1 = 4  # 定义右边第一个循迹红外传感器引脚为4口
TrackSensorRightPin2 = 18  # 定义右边第二个循迹红外传感器引脚为18口

# 光敏电阻引脚定义
LdrSensorLeft = 7
LdrSensorRight = 6

# 小车速度变量
CarSpeedControl = 30

# 设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)
# 忽略警告信息
GPIO.setwarnings(False)


# 电机引脚初始化为输出模式
# 按键引脚初始化为输入模式
# 超声波,RGB三色灯,舵机引脚初始化
# 红外避障引脚初始化
def init():
    global pwm_ENA
    global pwm_ENB
    global pwm_FrontServo
    global pwm_VerticalServo
    global pwm_HorizontalServo
    global pwm_rled
    global pwm_gled
    global pwm_bled
    GPIO.setup(ENA, GPIO.OUT, initial=GPIO.HIGH)
    GPIO.setup(IN1, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(IN2, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(ENB, GPIO.OUT, initial=GPIO.HIGH)
    GPIO.setup(IN3, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(IN4, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(buzzer, GPIO.OUT, initial=GPIO.HIGH)
    GPIO.setup(OutfirePin, GPIO.OUT, initial=GPIO.HIGH)
    GPIO.setup(EchoPin, GPIO.IN)
    GPIO.setup(TrigPin, GPIO.OUT)
    GPIO.setup(LED_R, GPIO.OUT)
    GPIO.setup(LED_G, GPIO.OUT)
    GPIO.setup(LED_B, GPIO.OUT)
    GPIO.setup(FrontServoPin, GPIO.OUT)
    GPIO.setup(ServoVerticalPin, GPIO.OUT)
    GPIO.setup(ServoHorizontalPin, GPIO.OUT)
    GPIO.setup(AvoidSensorLeft, GPIO.IN)
    GPIO.setup(AvoidSensorRight, GPIO.IN)
    GPIO.setup(LdrSensorLeft, GPIO.IN)
    GPIO.setup(LdrSensorRight, GPIO.IN)
    GPIO.setup(TrackSensorLeftPin1, GPIO.IN)
    GPIO.setup(TrackSensorLeftPin2, GPIO.IN)
    GPIO.setup(TrackSensorRightPin1, GPIO.IN)
    GPIO.setup(TrackSensorRightPin2, GPIO.IN)
    # 设置pwm引脚和频率为2000hz
    pwm_ENA = GPIO.PWM(ENA, 2000)
    pwm_ENB = GPIO.PWM(ENB, 2000)
    pwm_ENA.start(0)
    pwm_ENB.start(0)
    # 设置舵机的频率和起始占空比
    pwm_FrontServo = GPIO.PWM(FrontServoPin, 50)
    pwm_VerticalServo = GPIO.PWM(ServoVerticalPin, 50)
    pwm_HorizontalServo = GPIO.PWM(ServoHorizontalPin, 50)
    pwm_FrontServo.start(0)
    pwm_VerticalServo.start(0)
    pwm_HorizontalServo.start(0)
    pwm_rled = GPIO.PWM(LED_R, 1000)
    pwm_gled = GPIO.PWM(LED_G, 1000)
    pwm_bled = GPIO.PWM(LED_B, 1000)
    pwm_rled.start(0)
    pwm_gled.start(0)
    pwm_bled.start(0)


# 小车前进
def run():
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(CarSpeedControl)
    pwm_ENB.ChangeDutyCycle(CarSpeedControl)


# 小车后退
def back():
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(CarSpeedControl)
    pwm_ENB.ChangeDutyCycle(CarSpeedControl)


# 小车左转
def left():
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(CarSpeedControl)
    pwm_ENB.ChangeDutyCycle(CarSpeedControl)


# 小车右转
def right():
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(CarSpeedControl)
    pwm_ENB.ChangeDutyCycle(CarSpeedControl)


# 小车原地左转
def spin_left():
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(CarSpeedControl)
    pwm_ENB.ChangeDutyCycle(CarSpeedControl)


# 小车原地右转
def spin_right():
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(CarSpeedControl)
    pwm_ENB.ChangeDutyCycle(CarSpeedControl)


# 小车停止
def brake():
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)


# 超声波测距函数
def Distance_test():
    global start_time, t1, t2
    start_time = time.time()

    GPIO.output(TrigPin, GPIO.HIGH)
    time.sleep(0.000015)
    GPIO.output(TrigPin, GPIO.LOW)
    # 发送一次一次很短的超声波，监测收到高电平的时间
    while True:
        # 超过指定时间未监测到，则说明传感器异常
        if time.time() - start_time > 1:
            return -1
        if not GPIO.input(EchoPin):
            t1 = time.time()

    while True:
        if time.time() - start_time > 1:
            return -1
        if GPIO.input(EchoPin):
            t2 = time.time()
            break

    distance = ((t2 - t1) * 340 / 2) * 100
    print("distance is %d " % distance)
    return distance


# 前舵机旋转到指定角度
def frontservo_appointed_detection(pos):
    for i in range(18):
        pwm_FrontServo.ChangeDutyCycle(2.5 + 10 * pos / 180)
        time.sleep(0.02)  # 等待20ms周期结束
    # pwm_FrontServo.ChangeDutyCycle(0)  # 归零信号


# 摄像头舵机左右旋转到指定角度
def horizontal_servo_appointed_detection(pos):
    for i in range(1):
        pwm_HorizontalServo.ChangeDutyCycle(2.5 + 10 * pos / 180)
        time.sleep(0.02)  # 等待20ms周期结束
    # pwm_HorizontalServo.ChangeDutyCycle(0)  # 归零信号


# 摄像头舵机上下旋转到指定角度
def vertical_servo_appointed_detection(pos):
    for i in range(1):
        pwm_VerticalServo.ChangeDutyCycle(2.5 + 10 * pos / 180)
        time.sleep(0.02)  # 等待20ms周期结束
    # pwm_VerticalServo.ChangeDutyCycle(0)  # 归零信号


# 巡线测试
def tracking_test():
    # 检测到黑线时循迹模块相应的指示灯亮，端口电平为LOW
    # 未检测到黑线时循迹模块相应的指示灯灭，端口电平为HIGH
    TrackSensorLeftValue1 = GPIO.input(TrackSensorLeftPin1)
    TrackSensorLeftValue2 = GPIO.input(TrackSensorLeftPin2)
    TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
    TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
    infrared_track_value_list = ['0', '0', '0', '0']
    infrared_track_value_list[0] = str(1 ^ TrackSensorLeftValue1)
    infrared_track_value_list[1] = str(1 ^ TrackSensorLeftValue2)
    infrared_track_value_list[2] = str(1 ^ TrackSensorRightValue1)
    infrared_track_value_list[3] = str(1 ^ TrackSensorRightValue2)
    infrared_track_value = ''.join(infrared_track_value_list)
    return infrared_track_value


# 避障红外引脚测试
def infrared_avoid_test():
    # 遇到障碍物,红外避障模块的指示灯亮,端口电平为LOW
    # 未遇到障碍物,红外避障模块的指示灯灭,端口电平为HIGH
    LeftSensorValue = GPIO.input(AvoidSensorLeft)
    RightSensorValue = GPIO.input(AvoidSensorRight)
    infrared_avoid_value_list = ['0', '0']
    infrared_avoid_value_list[0] = str(1 ^ LeftSensorValue)
    infrared_avoid_value_list[1] = str(1 ^ RightSensorValue)
    infrared_avoid_value = ''.join(infrared_avoid_value_list)
    return infrared_avoid_value


# 寻光引脚测试
def follow_light_test():
    # 遇到光线,寻光模块的指示灯灭,端口电平为HIGH
    # 未遇光线,寻光模块的指示灯亮,端口电平为LOW
    LdrSersorLeftValue = GPIO.input(LdrSensorLeft)
    LdrSersorRightValue = GPIO.input(LdrSensorRight)
    LDR_value_list = ['0', '0']
    LDR_value_list[0] = str(LdrSersorLeftValue)
    LDR_value_list[1] = str(LdrSersorRightValue)
    LDR_value = ''.join(LDR_value_list)
    return LDR_value


# 小车鸣笛
def whistle():
    GPIO.output(buzzer, GPIO.LOW)
    time.sleep(0.1)
    GPIO.output(buzzer, GPIO.HIGH)
    time.sleep(0.001)


# 七彩灯亮指定颜色
def color_led_pwm(iRed, iGreen, iBlue):
    v_red = (100 * iRed) / 255
    v_green = (100 * iGreen) / 255
    v_blue = (100 * iBlue) / 255
    pwm_rled.ChangeDutyCycle(v_red)
    pwm_gled.ChangeDutyCycle(v_green)
    pwm_bled.ChangeDutyCycle(v_blue)
    time.sleep(0.02)


# 摄像头舵机向上运动
def servo_up():
    global ServoVerticalPos
    ServoVerticalPos += 0.7
    vertical_servo_appointed_detection(ServoVerticalPos)
    # time.sleep(0.05)
    if ServoVerticalPos >= 180:
        ServoVerticalPos = 180


# 摄像头舵机向下运动
def servo_down():
    global ServoVerticalPos
    ServoVerticalPos -= 0.7
    vertical_servo_appointed_detection(ServoVerticalPos)
    # time.sleep(0.05)
    if ServoVerticalPos <= 45:
        ServoVerticalPos = 45


# 摄像头舵机向左运动
def servo_left():
    global ServoHorizontalPos
    ServoHorizontalPos += 0.7
    horizontal_servo_appointed_detection(ServoHorizontalPos)
    # time.sleep(0.10)
    if ServoHorizontalPos >= 180:
        ServoHorizontalPos = 180


# 摄像头舵机向右运动
def servo_right():
    global ServoHorizontalPos
    ServoHorizontalPos -= 0.7
    horizontal_servo_appointed_detection(ServoHorizontalPos)
    # time.sleep(0.10)
    if ServoHorizontalPos <= 0:
        ServoHorizontalPos = 0


# 前舵机向左
def front_servo_left():
    frontservo_appointed_detection(180)


# 前舵机向右
def front_servo_right():
    frontservo_appointed_detection(0)


# 所有舵机归位
def servo_init():
    servoflag = 0
    servoinitpos = 90
    if servoflag != servoinitpos:
        frontservo_appointed_detection(servoinitpos)
        vertical_servo_appointed_detection(servoinitpos)
        horizontal_servo_appointed_detection(servoinitpos)
        time.sleep(0.5)
        pwm_FrontServo.ChangeDutyCycle(0)  # 归零信号
        pwm_HorizontalServo.ChangeDutyCycle(0)  # 归零信号
        pwm_VerticalServo.ChangeDutyCycle(0)  # 归零信号


# 摄像头舵机上下归位
def servo_vertical__init():
    vertical_servo_appointed_detection(90)


# 舵机停止
def servo_stop():
    pwm_HorizontalServo.ChangeDutyCycle(0)  # 归零信号
    pwm_VerticalServo.ChangeDutyCycle(0)  # 归零信号
    pwm_FrontServo.ChangeDutyCycle(0)  # 归零信号


# 灭火器状态切换
def out_fire():
    # False 为转动，True为停止
    new_status = not GPIO.input(OutfirePin)
    GPIO.output(OutfirePin, new_status)


def stop():
    pwm_ENA.stop()
    pwm_ENB.stop()
    pwm_rled.stop()
    pwm_gled.stop()
    pwm_bled.stop()
    pwm_FrontServo.stop()
    pwm_HorizontalServo.stop()
    pwm_VerticalServo.stop()
    GPIO.cleanup()
